#include "motor_test.h"
#include "mc_config.h"
#include "mc_tasks.h"
#include "mc_interface.h"
#include "shell.h"

motorTestcmd_t motorTestCMD =
{
    .speedSqure = 0,
    .tourqeSqure = 0,
    .speedSine = 0,
    .tourqeSine = 0,
};

extern MCI_Handle_t * oMCInterface[NBR_OF_MOTORS];

//this function is exec at systick irq handle
void motorSpeedSqure_Test(void)
{
    static uint32_t count;
    uint8_t sumOfCMD;
    sumOfCMD = motorTestCMD.speedSine + motorTestCMD.tourqeSine + motorTestCMD.speedSqure + motorTestCMD.tourqeSqure;
    if(sumOfCMD != 1)
    {
        count = 0;
        return;
    }
    else if(motorTestCMD.speedSqure == 1)
    {
        count++;
        if(count == 4000)
        {
            // STC_SetControlMode(oMCInterface[M1]->pSTC, STC_SPEED_MODE);
            MCI_ExecSpeedRamp(oMCInterface[M1], 250, 400);//250*6 = 1500rad/min
        }
        else if(count == 8000)
        {
            // STC_SetControlMode(oMCInterface[M1]->pSTC, STC_SPEED_MODE);
            MCI_ExecSpeedRamp(oMCInterface[M1], -250, 400);//250*6 = 1500rad/min
            count = 0;
        }
    }
    else if(motorTestCMD.tourqeSqure == 1)
    {
        ;
    }
    else if(motorTestCMD.speedSine == 1)
    {
        ;
    }
    else if(motorTestCMD.tourqeSine == 1)
    {
        ;
    }
    else
    {
        ;
    }

}




